﻿using System;
using System.Collections.Generic;
using System.IO;
using System.Threading;

/// IMPORTANT
/// direct acces to disc!
/// Be carefull
using System.Runtime.InteropServices;
using Microsoft.Win32.SafeHandles;

using csmatio.types;
using csmatio.io;

namespace GroundStation.fdr
{
    /// <summary>
    /// This class represents acces to RAW data of Flight Data Recorder used as part of Modular System
    /// </summary>
    /// <author>Petr Novacek (petr.novacek@fel.cvut.cz)</author>>
    class SDlogger// : IDisposable
    {
        [DllImport("Kernel32.dll", SetLastError = true, CharSet = CharSet.Auto)]
        static extern SafeFileHandle CreateFile(
            string fileName,
            [MarshalAs(UnmanagedType.U4)] FileAccess fileAccess,
            [MarshalAs(UnmanagedType.U4)] FileShare fileShare,
            int securityAttributes,
            [MarshalAs(UnmanagedType.U4)] FileMode creationDisposition,
            [MarshalAs(UnmanagedType.U4)] FileAttributes fileAttributes,
            IntPtr template);

        private int _coreData = 39;//39;// WiFi 18, Core 39;//27; // count of core modules initially 39, possible to lower to 31
        private int _maxSudents = 20;//20; // 0 for wifi, 20 for onboard // maximum of student modules
        private bool _wifi_log = false;
        private string _finName; // input file name
        private bool _dSDc;
        private string _startTime; // formated start time stamp
        private Thread ReadThread;
        private float _time_old; // time vector change
        private bool _run = false;

        protected bool _directSD = false;
        protected SafeFileHandle inputHandle;


        /// <summary>
        /// Construct the object of Flight Data Recorded (FDR) RAW data acces
        /// </summary>
        /// <param name="file">Input file name</param>
        /// <param name="dSDc">bool sSDc - true if load data directly from SD card</param>
        public SDlogger(string file, bool dSDc)
        {
            if (0 == SDinit(file, dSDc))
            {
                //do sometning
            }
            this.ReadThread = new Thread(new ThreadStart(GetSDFrames));
        }

        /// <summary>
        /// SD logger initialization function
        /// </summary>
        /// <param name="file">string representin file name of SD card drive letter</param>
        /// <param name="dSDc">bool - true if data are loaded directly from SD card</param>
        /// <returns></returns>
        private int SDinit(string file, bool dSDc)
        {
            string fn;
            if (dSDc)
            {
                fn = @"\\.\" + file.Substring(0, 1) + ":";
            }
            else
            {
                fn = @file;
            }
            this._finName = fn;
            this._dSDc = dSDc;
            this._startTime = DateTime.Now.ToString("yyyy-MM-dd_HHmmss");
            inputHandle = CreateFile(_finName,
            FileAccess.ReadWrite,
            FileShare.ReadWrite, // drives must be opened with read and write share access
            0,
            FileMode.Open,
            FileAttributes.Normal,
            IntPtr.Zero);
            if (inputHandle.IsInvalid)
            {
                throw new Exception("Invalid Safe Handle."); //Win32Exception(Marshal.GetLastWin32Error());
            }

            this.ReadThread = new Thread(new ThreadStart(GetSDFrames));
            this.ReadThread.Start();
            this._run = true;
            return 0;
        }

        public bool SDIsRunning()
        {
            return _run;
        }

        public bool SDStop()
        {
            if (this._run)
            {
                this._run = false;
            }
            return true;
        }

        /// <summary>
        /// Function which gets data from file/SD card and store them in .mat file array
        /// </summary>
        protected void GetSDFrames()
        {
            List<float[]> mdata = new List<float[]>();
            int buff_len = 512;
            int d_buff_len = 239;
            int data_len = 0;
            int index; ;
            int headSD = 0;
            byte[] buff = new byte[buff_len];
            byte[] d_buff = new byte[d_buff_len];
            int d_index = 0;
            int msg_count = 0;
            int fault_msgs = 0;
            int frame_coutn = 0;
            bool new_data = true;
            byte checksum = 0;
            int zeroheadcount = 0;
            //int sdSect = 4194304;
            //bool ignore = true;
            try
            {
                using (FileStream InStream = new FileStream(inputHandle, FileAccess.Read))
                {
                    while (new_data & this._run) // for (int i = 0; i < 10; i++)//frame_coutn<sdSect
                    {
                        index = 0; // SD sector index
                        InStream.Read(buff, 0, buff.Length);
                        headSD = ConvertUint16Data(ref buff, index) & (ushort)0x3ff; // SD sector headder
                        if (headSD == 0) // header check
                        {
                            zeroheadcount++;
                            if (zeroheadcount > 200)
                            {
                                new_data = false;
                                break;
                            }
                        }
                        else if (zeroheadcount != 0)
                            zeroheadcount = 0;//*/
                        frame_coutn++; // SD card frame counter
                        index += 2; // start of data in SD frame

                        while (index < headSD) // repead for whole SD frame
                        {
                            d_buff[d_index] = buff[index++]; // copy SD buffer byte into GS message buffer byte // index++ problem?
                            if (d_index == 0) // first GS message byte
                            {
                                if (d_buff[d_index] == 171) // check apropriate header
                                {
                                    d_index++; // count up
                                }
                            }
                            else if (d_index == 1) // GS message lenght
                            {
                                data_len = d_buff[d_index] + 2; // copy lenght into apropriate variable
                                if (msg_count==0 && data_len<_coreData*4)
                                {
                                    _coreData = 18;
                                    _maxSudents = 0;
                                    _wifi_log = true;
                                }
                                if ((data_len - 2) <= (_coreData + _maxSudents) * 4) // check!!!
                                {
                                    //if (data_len > d_buff_len)
                                    //    data_len = d_buff_len;
                                    d_index++; // continue with message
                                }
                                else
                                {
                                    //data_len = (_coreData + _maxSudents)*4;
                                    //System.Console.WriteLine("Message too long");
                                    //throw new Exception("Message too long");
                                    //d_index++;
                                    d_index = 0;
                                    checksum = 0;
                                }
                            }
                            else if (d_index < data_len)
                            {
                                checksum += d_buff[d_index];
                                d_index++;
                            }
                            else
                            {
                                if (checksum == d_buff[d_index])
                                {
                                    float[] mdatarow = GetData(ref d_buff, data_len);
                                    mdata.Add(mdatarow); // process the message
                                    msg_count++; // count the message
                                }
                                else // possibility of next message loose
                                {
                                    //System.Console.WriteLine();
                                    fault_msgs++;
                                }
                                d_index = 0;
                                checksum = 0;
                            }
                        }
                    }
                }
            }
            catch (Exception e)
            {
                FunctionsOut(e.ToString());
            }
            if (mdata.Count > 0)
            {
                try
                {
                    float[][] mlData = mdata.ToArray(); // list to array conversion - possible improvement in future
                    MLSingle mlSingle = new MLSingle("data", mlData); // creates matlab array of single values as "data" variable
                    List<MLArray> list = new List<MLArray>(); // list of matlab array
                    list.Add(mlSingle); // adding data to matlab list
                    string logName_app = "";
                    if (_wifi_log)
                    {
                        logName_app = "SD_WiFi_";
                    }
                    else
                    {
                        logName_app = "SD_OnBoard_";
                    }
                    new MatFileWriter(logName_app + this._startTime + ".mat", list, false); // creates .mat file: string filename, array data, bool compression
                    //System.Windows.Forms.MessageBox.Show("Data successfuly read","SD Card",System.Windows.Forms.MessageBoxButtons.OK);
                    StreamWriter logout = new StreamWriter("log.out", true);
                    
                    logout.WriteLine("SD card at " + DateTime.Now.ToString("yyyy-MM-dd (HH:mm:ss)") + ": read " + msg_count.ToString() +
                         " messages from " + frame_coutn.ToString() + " sectors with " + fault_msgs.ToString() + " messages with wrong checksum.");
                    logout.Close();
                }
                catch (System.OutOfMemoryException e)
                {
                    System.Windows.Forms.MessageBox.Show(e.Message, "Out of memmory", System.Windows.Forms.MessageBoxButtons.OK);
                }
            }
            if (this._run)
                this._run = false;
        }

        /// <summary>
        /// Single SDlogger message parser
        /// </summary>
        /// <param name="d_buff">ref byte[] d_buff - message buffer</param>
        /// <param name="data_len">int data_len - message lenght</param>
        /// <returns>float[] dataConvert - array of message data</returns>
        private float[] GetData(ref byte[] d_buff, int data_len)
        {

            int index = 0;
            float[] dataConvert = new float[_coreData + _maxSudents];
            byte dataHead = 0;  // telemetry message data header 0xAB
            byte dataLength = 0; // telemetry message data length 140
            try
            {
                dataHead = d_buff[index++];
                dataLength = d_buff[index++];
                dataConvert[0] = ConvertUint32Data(ref d_buff, index);  // time in msecs
                index += 4; // index increment
                if(_wifi_log)
                    dataConvert[1] = ConvertFloatData(ref d_buff, index); // WiFi Log
                else
                    dataConvert[1] = ConvertUint32Data(ref d_buff, index); // OnBoard log
                index += 4; // index increment
                _time_old = dataConvert[0];
                int i = 0;
                for (i = 2; i < _coreData; i++)    // cycle to prepare string line
                {
                    dataConvert[i] = ConvertFloatData(ref d_buff, index); // telemetry message float conversion
                    index += 4; // index increment
                }

                if (data_len > (_coreData * 4)) // student data
                {
                    int length = data_len;
                    //int ind = 0;
                    d_buff[length] = 0; // checksum erase

                    //if (length > (_coreData + _maxSudents) * 4) length = (_coreData + _maxSudents) * 4;
                    for (int j = 0; j <= length - (_coreData * 4); j += 4)
                    {
                        dataConvert[i++] = ConvertFloatData(ref d_buff, (_coreData * 4) + j + 2);// add student data to Matlab float vector - will be saved to .mat file
                        //ind++;
                    }

                }
            }
            catch (Exception e)
            {
                System.Console.WriteLine(e.Message);
            }
            return dataConvert;
        }

        /// <summary>
        /// Convert raw data input into 32bit unsignet integer
        /// </summary>
        /// <param name="buf">ref byte[] - data buffer</param>
        /// <param name="index">int - index of unsignet int place in buffer</param>
        /// <returns>UInt32 - 32bit unsignet ingeger value</returns>
        private UInt32 ConvertUint32Data(ref byte[] buf, int index)
        {
            byte[] val = new byte[4];
            UInt32 ret = 0;
            try
            { // reverse order for HC12, NOT reversing for 407!!! The most stupid PapajBug I've found ;)
                /*val[0] = buf[index + 3];
                val[1] = buf[index + 2];
                val[2] = buf[index + 1];
                val[3] = buf[index + 0];//*/
                val[0] = buf[index + 0];
                val[1] = buf[index + 1];
                val[2] = buf[index + 2];
                val[3] = buf[index + 3];//*/
                ret = BitConverter.ToUInt32(val, 0);
            }
            catch (Exception e)
            {
                FunctionsOut(e.ToString());
            }
            return ret;
        }

        /// <summary>
        /// Convert raw data input into floating point number
        /// </summary>
        /// <param name="buf">ref byte[] - data buffer</param>
        /// <param name="index">int - index of float place in buffer</param>
        /// <returns>float - floating point number</returns>
        private float ConvertFloatData(ref byte[] buf, int index)
        {
            byte[] val = new byte[4];
            float ret = 0;
            try
            {
                /*val[0] = buf[index + 3];
                val[1] = buf[index + 2];
                val[2] = buf[index + 1];
                val[3] = buf[index + 0];//*/
                val[0] = buf[index + 0];
                val[1] = buf[index + 1];
                val[2] = buf[index + 2];
                val[3] = buf[index + 3];//*/
                ret = BitConverter.ToSingle(val, 0);
            }
            catch (Exception e)
            {
                FunctionsOut(e.ToString());
            }
            return ret;
        }

        /// <summary>
        /// Convert raw data input into 16bit unsignet integer
        /// </summary>
        /// <param name="buf">ref byte[] - data buffer</param>
        /// <param name="index">int - index of unsignet int place in buffer</param>
        /// <returns>UInt16 - 32bit unsignet ingeger value</returns>
        private UInt16 ConvertUint16Data(ref byte[] buf, int index)
        {
            byte[] val = new byte[2];
            UInt16 ret = 0;
            try
            {
                val[0] = buf[index + 1];
                val[1] = buf[index + 0];
                ret = BitConverter.ToUInt16(val, 0);
            }
            catch (Exception e)
            {
                FunctionsOut(e.ToString());
            }
            return ret;
        }

        private int FunctionsOut(string line) // log function - easy change to frame application
        {
            Console.WriteLine(line);
            return 0;
        }
    }
}
